/*
 * SensorPoller.cpp
 *
 *  Created on: 4 dec. 2012
 *      Author: Admin
 */

#include "Robot.hpp"

#include "CollisionEvent.hpp"
#include "EmergencyStopActivatedEvent.hpp"
#include "EmergencyStopDisabledEvent.hpp"
#include "EnteredColorLaneEvent.hpp"
#include "EnteredCrossRoadsEvent.hpp"
#include "LeftColorLaneEvent.hpp"
#include "ProximityEvent.hpp"

#include "SensorPoller.hpp"
#include "CollisionSensor.hpp"
#include "ColorLaneSensor.hpp"
#include "CrossRoadsSensor.hpp"
#include "EmergencyStopSensor.hpp"

SensorPoller::SensorPoller() : sensorCount(0) { }
SensorPoller::~SensorPoller() { }

void SensorPoller::poll()
{
	for (short i = 0; i < sensorCount; i++)
	{
		//Note!! (*it) points towards pointer to sensor!!!

		//Get sensor pointer
		Sensor* sensorPointer = sensors[i];

		//Get last and new value of this sensor
		bool 	lastValue = lastValues[i],
				newValue = sensorPointer->poll();

		//Do some casting stuff
		switch (sensorPointer->getSensorType())
		{
			case Sensor::COLLISION:
			{
				Side side = ((CollisionSensor*) sensorPointer)->getSide();
				pollCollisionSensor(side ,lastValue, newValue);
				break;
			}
			case Sensor::COLORLANE:
			{
				pollColorLaneSensor(lastValue, newValue);
				break;
			}
			case Sensor::CROSSROADS:
			{
				pollCrossRoadsSensor(lastValue, newValue);
				break;
			}
			case Sensor::DISTANCE:
			{
				pollDistanceSensor(lastValue, newValue);
				break;
			}
			case Sensor::EMERGENCYSTOP:
			{
				pollEmergencyStopSensor(lastValue, newValue);
				break;
			}
		}

		//Save the new value as last value
		lastValues[i] = newValue;
	}
}

void SensorPoller::addSensor(Sensor* sensor)
{
	if (sensorCount == 10) //Max 10 sensors allowed
		return;
	lastValues[sensorCount] = sensor->poll();
	sensors[sensorCount++] = sensor;
}

void SensorPoller::addSensor(Sensor& sensor)
{
	if (sensorCount == 10) //Max 10 sensors allowed
		return;
	lastValues[sensorCount] = sensor.poll();
	sensors[sensorCount++] = &sensor;
}

void SensorPoller::pollCollisionSensor(Side side, bool lastValue, bool newValue)
{
	if (newValue == 1) //(existing) collision occurred
	{
		CollisionEvent event(side);
		Robot::getInstance().handleEvent(event);
	}
}

void SensorPoller::pollColorLaneSensor(bool lastValue, bool newValue)
{
	if (lastValue == 0 && newValue == 1) //just entered color lane
	{
		EnteredColorLaneEvent event;
		Robot::getInstance().handleEvent(event);
	}
	else if (lastValue == 1 && newValue == 0) //just left color lane
	{
		LeftColorLaneEvent event;
		Robot::getInstance().handleEvent(event);
	}
}

void SensorPoller::pollCrossRoadsSensor(bool lastValue, bool newValue)
{
	if (lastValue == 0 && newValue == 1) //just entered crossroads
	{
		EnteredCrossRoadsEvent event;
		Robot::getInstance().handleEvent(event);
	}
	else if (lastValue == 1 && newValue == 0) //just left crossroads
	{
		//Currently not a event
	}
}

void SensorPoller::pollDistanceSensor(bool lastValue, bool newValue)
{
	if (newValue == 1) //(existing) collision occurred
	{
		ProximityEvent event;
		Robot::getInstance().handleEvent(event);
	}
}

void SensorPoller::pollEmergencyStopSensor(bool lastValue, bool newValue)
{
	if (lastValue == 0 && newValue == 1) //emergency stop was just activated
	{
		EmergencyStopActivatedEvent event;
		Robot::getInstance().handleEvent(event);
	}
	else if (lastValue == 1 && newValue == 0) //emergency stop was just disabled
	{
		EmergencyStopDisabledEvent event;
		Robot::getInstance().handleEvent(event);
	}
}
